//============================================================================
// Name        : localizadorWAI.cpp
// Author      : Milton Yukio Godoy Saito
// Version     : 0.01
// Copyright   : WAI2008
// Description : Localizador WAI baseado no WBLS, Ansi-style
//============================================================================

#include "localizadorWAI.h"

using namespace std;

void inicia_vetores(){
  for (int i=0; i<N_PM; i++) {
    Ppm[i] = 0;
    Ppm_ant[i] = 1;
  }
}
void carrega_mapa(){

   // Connect to the database.
  if (!con.connect(BANCO_DE_DADOS,"localhost", "root", "letmein")) {
    cout << "Problemas ao conectar com o Banco de Dados";
    exit(0);
  }
  query = con.query();

  // carregando coordenadas dos PMs
  cout << "carregando coordenadas dos PMs ..." << endl;
  query << "select id, x, y, z from PMs;";
  res2 = query.store();
  for (int i=0; i<res2.num_rows(); i++){
    int pm = atoi ( res2.at(i).at(0).get_string().c_str() );
    PM[pm].x = atof ( res2.at(i).at(1).get_string().c_str() );
    PM[pm].y = atof ( res2.at(i).at(2).get_string().c_str() );
    PM[pm].floor = atoi ( res2.at(i).at(3).get_string().c_str() );
  }


  // carregando a tabela PAs
  cout << "carregando a tabela PAs ..." << endl;
  query << "select id, ssid from PAs;";
  res2 = query.store();
  for (int i=0; i<res2.num_rows(); i++){
    PA[i].id = atoi ( res2.at(i).at(0).get_string().c_str() );

     strcpy (PA[i].ssid,res2.at(i).at(1).get_string().c_str());
    //PA[i].ssid = res2.at(i).at(1).get_string();
  }

  // carregando Histogramas
  cout << "carregando Histogramas ..." << endl;
  for (int pm=0; pm<N_PM; pm++){
     for (int pa=0; pa<N_PA; pa++){
       query << "select namostras, presente, media, desvio from Histogramas where pm = " << pm << " and pa = " << pa << ";";
       res2 = query.store();
       if (res2.num_rows()!=0){
         H[pm][pa].total = atoi( res2.at(0).at(0).get_string().c_str() ); // total
         H[pm][pa].presente = atoi( res2.at(0).at(1).get_string().c_str() ); // presente
         H[pm][pa].media = atof( res2.at(0).at(2).get_string().c_str() ); // media
         H[pm][pa].desvio = atof( res2.at(0).at(3).get_string().c_str() ); // desvio
       }
     }
  }

   // carregando Arestas
   cout << "carregando Arestas ..." << endl;
   for (int pm=0; pm<N_PM; pm++){
     query << "select pm1 from Arestas where pm2 = " << pm << ";";
     res2 = query.store();
     n_Cel_Adj[pm] = res2.num_rows(); // no. de adjacentes
     for(int i=0; i<n_Cel_Adj[pm]; i++) {
       Cel_Adj[pm][i].id = atoi( res2.at(i).at(0).get_string().c_str() );
       query << "select count(pm2) from Arestas where pm1 = " << Cel_Adj[pm][i].id  << ";";
       res3 = query.store();
       Cel_Adj[pm][i].n_arestas = atoi( res3.at(0).at(0).get_string().c_str() );
     }
   }

   // carregando Histograma de Ruídos
   cout << "carregando Histograma de Ruídos ..." << endl;
   for (int sinal=0; sinal<RSSI_MAX; sinal++){
     HR[sinal] = Beta;
     HR_count += Beta;
   }
   query << "select sinal, contador from histograma_ruido;";
   res2 = query.store();
   for(int i=0; i<res2.num_rows(); i++) {
     int sinal = atoi( res2.at(i).at(0).get_string().c_str() );
     int contador = atoi( res2.at(i).at(1).get_string().c_str() );
     HR[sinal] += contador;
     HR_count += contador;
   }

   // carregando Máscara de PAs
   cout << "carregando Máscara de PAs ..." << endl;
   query << "select pm, pa from mascara_PAs;";
   res2 = query.store();
   for(int i=0; i<res2.num_rows(); i++) {
     int pm = atoi( res2.at(i).at(0).get_string().c_str() );
     int pa = atoi( res2.at(i).at(1).get_string().c_str() );
     mascara_PAs[pm][pa] = 1;
   }

   cout << "Banco de Dados carregado."<< endl;
}

double calcula_somatoria_transicao(int pm){

  double p = 0;

  // cálculo da auto-transição
  p += Ppm_ant[pm]*PPC;

  // cálculo da outras transições possíveis
  int num = n_Cel_Adj[pm];
  for(int i=0; i<num; i++) {
    int pm_origem = Cel_Adj[pm][i].id;
    double pt = Ppm_ant[pm_origem]*( (1-PPC)/Cel_Adj[pm][i].n_arestas );
    p += pt;
    //cout << "P(vir de " << pm_origem << ") = " << pt << endl;
  }
  //cout << "P_trans = " << p << endl;
  return p;
}

double calcula_prob_obs(scan Medida, int pm){

  double p = 1;
  double prob;

  for (int pa=0; pa<N_PA; pa++){
    if (valido[pa] == 1) {
      int sinal = Medida.sinal[pa];
      int total = H[pm][pa].total;
      int presente = H[pm][pa].presente;
      float media = H[pm][pa].media;
      float desvio = H[pm][pa].desvio;


      if (media==0) media = 97;
      if (desvio==0) desvio = 0.1;

      if (sinal != 0) {
          double N = (normal_integral(media, desvio,RSSI_MIN,RSSI_MAX)) + (RSSI_MAX-RSSI_MIN)*Beta;
          double s = (double) sinal;
          prob = (normal_integral(media, desvio,s-0.5,s+0.5)+Beta) / N;
          p = p * prob;
      }

    }
  }
  return p;
}


coordenada estima(scan Medida){

	double Pmax = 0;
	double soma = 0;
	double px = 0;
	double py = 0;
	double Pobs,somaPtrans;
	coordenada result;
	float x,y;
	int nuser;
	bool  achou = false;

	for (nuser = 0;nuser < nUsuarios;nuser++){
		if (Medida.hdSerial == probAntByUser[nuser].hdSerial){
			achou = true;
			break;
		}
	}
	if(achou){
		cout << "user number: "  << nuser << endl;
		for (int j=0; j<N_PM; j++) {
			Ppm[j] = 0;
			Ppm_ant[j] = probAntByUser[nuser].Ppm_ant[j];
		}

	}else{
		cout << "new user!"<< endl;
		nUsuarios++;
		probAntByUser[nuser].hdSerial = Medida.hdSerial;
		for (int j=0; j<N_PM; j++) {
			Ppm[j] = 0;
			Ppm_ant[j] = 1;
		}
	}

	for (int pm=0; pm<N_PM; pm++){

		if (PM[pm].floor == FLOOR){
			x = PM[pm].x;
			y = PM[pm].y;

			Pobs = calcula_prob_obs(Medida, pm);
			somaPtrans = calcula_somatoria_transicao(pm);

			Ppm[pm] = Pobs*somaPtrans;
			soma += Ppm[pm];

			if (Ppm[pm] > Pmax){
				Pmax = Ppm[pm];
			}
			px += x * Ppm[pm];
			py += y * Ppm[pm];
		}
	}

	result.x = px/soma;
	result.y = py/soma;

	// prepara para as próximas iterações
	for (int i=0; i<N_PM; i++){
		Ppm_ant[i] = Ppm[i]/soma;
		Ppm[i] = 0;
	}

	if(achou){
		for (int j=0; j<N_PM; j++) {
			probAntByUser[nuser].Ppm_ant[j] = Ppm_ant[j];
		}
	}else{
		for (int j=0; j<N_PM; j++){
			probAntByUser[nuser].Ppm_ant[j] = Ppm_ant[j];
		}
	}
	return result;
}

scan convStr2Scan(string rssis){
	scan result;
	string tagIniSSProtocol = "<SSProtocol>";
	string tagFimSSProtocol = "</SSProtocol>";
	string tagIniHDSerial = "<HDSerial>";
	string tagFimHDSerial = "</HDSerial>";
	string tagIniNetwork = "<Network>";
	string tagFimNetwork = "</Network>";
	string tagIniID = "<ID>";
	string tagFimID = "</ID>";
	string tagIniSignal = "<Signal>";
	string tagFimSignal = "</Signal>";

	if(rssis.substr(0,tagIniSSProtocol.length()) == tagIniSSProtocol){ // tem a tag root
		rssis = rssis.substr(tagIniSSProtocol.length());
		//  tag HDSerial
		if(rssis.substr(0,tagIniHDSerial.length()) == tagIniHDSerial){
			rssis = rssis.substr(tagIniHDSerial.length());
			int hdSerialStrLength = rssis.find(tagFimHDSerial);

			string hdSerial(rssis,0,hdSerialStrLength);
			cout <<"HDSerial: " <<hdSerial << endl;
			result.hdSerial = atoi(hdSerial.c_str());

			rssis = rssis.substr(hdSerialStrLength+ tagFimHDSerial.length());
		}
		// tags Network

		while (rssis != tagFimSSProtocol){
			string id, signal;
			if(rssis.substr(0,tagIniNetwork.length()) == tagIniNetwork){
				rssis = rssis.substr(tagIniNetwork.length());
				//  tag ID
				if(rssis.substr(0,tagIniID.length()) == tagIniID){
					rssis = rssis.substr(tagIniID.length());
					int idStrLength = rssis.find(tagFimID);
					id = rssis.substr(0,idStrLength);
					rssis = rssis.substr(idStrLength+ tagFimID.length());
				}
				//  tag Signal
				if(rssis.substr(0,tagIniSignal.length()) == tagIniSignal){
					rssis = rssis.substr(tagIniSignal.length());
					int signalStrLength = rssis.find(tagFimSignal);
					signal = rssis.substr(0,signalStrLength);
					rssis = rssis.substr(signalStrLength+ tagFimSignal.length());

				}
				cout <<"ID: " << id <<" Signal: " << signal << endl;
				rssis = rssis.substr(tagFimNetwork.length());

				for(int x=0; x<N_PA; x++){
					if (PA[x].ssid == id){
						result.sinal[PA[x].id] = atoi(signal.c_str());
					}else{
						result.sinal[x] = 0;
					}
				}
			}else{
				break;
			}
		}
	}
	return result;
}

void *waid(void* p){

	SWorkingThread* pData = (SWorkingThread*)p;
	cout << "\nThread #" << (pData->n_thread) << endl;
	try
	{
		string dataIN;
		string strOUT;
		char dataOUT[128];
		(*pData->m_pSocket) >> dataIN;
		//cout << dataIN << endl;

		scan Medida = convStr2Scan(dataIN);

		coordenada resposta = estima(Medida);

		if (resposta.x > 0 || resposta.y > 0){
			sprintf(dataOUT,"<SSProtocol><XEstimado>%f</XEstimado><YEstimado>%f</YEstimado></SSProtocol>\n",resposta.x,resposta.y);
		}else{
			sprintf(dataOUT,"<SSProtocol><ErrorCode>001</ErrorCode><ErrorDescription>negative position value</ErrorDescription></SSProtocol>\n",resposta.x,resposta.y);
		}
		strOUT= dataOUT;
		cout << "x: " << resposta.x << " y: " << resposta.y << endl;

		(*pData->m_pSocket) << strOUT;

	}catch ( SocketException& e ) {
		cout << "Exception was caught:" << e.description() << "\nExiting.\n";
	}
	pthread_exit(NULL);
}


int main(int argc, int argv[]){
	nUsuarios = 0;
	carrega_mapa();
	inicia_vetores();

	try
	{
		ServerSocket server (SERVER_PORT);
		int nthread=0;
		cout << "running..." << endl << endl;

		while (true){
			ServerSocket *new_sock = new ServerSocket();
			server.accept (*new_sock);

			/* create a new thread to process the incomming request */
			pthread_t* thread = new pthread_t();
			SWorkingThread* pInitData = new SWorkingThread(new_sock,nthread++);

			if (pthread_create(thread, NULL, waid,(void*) pInitData)){
				printf("ERROR; return code from pthread_create()\n");
				exit(-1);
			}
		}
	}catch ( SocketException& e ){
	  cout << "Exception was caught:" << e.description() << "\nExiting.\n";
	}
	return 0;
}



